#include "../include/SerialPort.hpp"
#include "../include/TransmitPacket.hpp"
#include "../include/ExplicitAddressingPacket.hpp"
#include "../include/util.hpp"

#include <unistd.h>

using std::string;
using std::cout;
using std::endl;
using boost::shared_ptr;



void wait_for_response (shared_ptr<Packet> response) {
    /* Listen for response, exit early when we have a valid checksum. */
    uint32_t count = 0;
//    while (!(response->is_valid()) && ++count < 50000) {
//        usleep(1);
//    }
    while (++count < 50000) {
        usleep(1);
    }
}


void print_packet(Packet* packet) {

    /* Output raw hex data. */
    for (uint8_t i = 0; i < packet->get_data()->size(); ++i) {
        cout << std::setw(2) << hex(packet->get_data()->at(i)) << " ";
    }

    cout << endl;

    /* Print a slightly cleaner translation of 'char' type bytes. */
    for (uint8_t i = 0; i < packet->get_data()->size(); ++i) {
        if (packet->get_data()->at(i) > 0x30 && packet->get_data()->at(i) < 0x7B) {
            cout << std::setfill(' ');
            cout << std::setw(2) << (char)(packet->get_data()->at(i)) << " ";
        } else {
            cout << std::setfill('0');
            cout << std::setw(2) << hex(packet->get_data()->at(i)) << " ";
        }
    }

    cout << endl;
}




int main(int argc, char** argv) {

    /* initialize serial port. */
    SerialPort serial;
    bool retval = serial.start("/dev/ttyAMA0", 9600);
    if (retval == false) {
        return (-1);
    }

    char packet_type;
    char node_num;
    string args;

    /* Stay in this loop until user types CTRL + c OR enters 'z' as the node number. */
    while (true) {

        std::cin.clear();

        /* Block while waiting for input. */
        (std::cin >> packet_type >> node_num >> args);


        /* Validate packet_type */
        if (packet_type != 't' && packet_type != 'e') {
            /* 't' for transmit, 'e' for explicit addressing */
            cout << "Error.  Invalid packet type.  Valid types are 't' & 'e'. ";
            cout << "'t' for transmit, 'e' for explicit addressing." << endl;
            continue;
        }


        /* Validate node number */
        if (node_num == 'z') {
            break;
        }

        if (node_num != 'b' && node_num != 'g') {
            cout << "Error.  Invalid node number.  Valid nodes are 'b' & 'g'. ";
            cout << "To exit, enter 'z' as the node number or type CTRL+c." << endl;
            continue;
        }

        /* If we're here, print the valid input. */
        cout << "type: " << packet_type << " node: " << node_num << " text: " << args << endl;


        uint32_t node;
        switch (node_num) {
        case 'b':
            node = Packet::node[0];
            break;
        case 'g':
            node = Packet::node[1];
            break;
        default:
            break;
        }

        /* Use the data which was passed in to build an outgoing packet. */
        shared_ptr<TransmitPacket> transmit_pkt(new TransmitPacket());
        shared_ptr<ExplicitAddressingPacket> e_pkt(new ExplicitAddressingPacket());
        uint8_t frame_id = 1; /* Just anything nonzero, for zero suppresses response. */

        switch (packet_type) {
            case 't':

                transmit_pkt->assemble_payload(++frame_id, node, args);
                transmit_pkt->createPacket();

                /* Send the constructed packet over UART TXD */
                cout << "Transmit: " << endl;
                for (uint8_t i = 0; i < transmit_pkt->get_data()->size(); ++i) {
                    serial.write(transmit_pkt->get_data()->at(i));
                }

                print_packet(transmit_pkt.get());
                break;

            case 'e':
                e_pkt->assemble_payload(++frame_id, node, args);
                e_pkt->createPacket();

                cout << "Explicit: " << endl;
                for (uint8_t i = 0; i < e_pkt->get_data()->size(); ++i) {
                    serial.write(e_pkt->get_data()->at(i));
                }

                print_packet(e_pkt.get());
                break;
            default:
                cout << "Bad packet type." << endl;
                break;
        };

        /* The SerialPort copies the received data into a Packet object using a shared pointer. */
        cout << "Receive: " << endl;
        shared_ptr<Packet> received(new Packet());
        serial.set_packet_rxd_ptr(&received);
        wait_for_response(received);

        print_packet(received.get());
    }

    return (0);
}
